﻿using System;
using System.Collections.Generic;

namespace GeoFits
{
    /// <summary>
    /// 圆心拟合
    /// </summary>
    public class CircleFit
    {
        /// <summary>
        /// 二维圆快速拟合，一般用于圆心拟合的初始值确定
        /// </summary>
        /// <param name="pts"><para>要进行拟合的点</para></param>
        /// <param name="P"><para>权阵</para></param>
        /// <param name="config"><para>是否使用抗差最小二乘法</para></param>
        /// <returns>拟合得到的圆</returns>
        /// <remarks>
        /// <para>注意：所谓快速拟合方法指的是用方程x*x+y*y+a*x+b*y+c=0来进行圆拟合。</para>
        /// <para>此方法运算速度快，但几何意义不明确。一般工程测量中使用精度足够</para>
        /// </remarks>
        public static FitResult Fit2DInFastMode(List<Point> pts, Matrix P = null, RLSConfig config = null)
        {
            //快速拟合的方法
            List<Point> newPts = new List<Point>();
            Vector moveVector = new Vector(pts[0].X, pts[0].Y, pts[0].Z);
            foreach (Point pt2 in pts)
            {
                newPts.Add(pt2 - moveVector);
            }
            FitResult result = new FitResult();

            Matrix X = FitPattern.LinearModule(newPts, CircleFitFunctionModule_FastMode, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, P, config);
            double z = 0;
            foreach (Point pt1 in newPts)
            {
                z += pt1.Z;
            }
            Point pt = new Point(-0.5 * X[0, 0], -0.5 * X[1, 0], z / newPts.Count) + moveVector;

            result.ResultGeometry = new Circle(pt, Math.Sqrt(Math.Abs((X[0, 0] * X[0, 0] + X[1, 0] * X[1, 0]) / 4 - X[2, 0])));
            return result;
        }
        /// <summary>
        /// 圆的平面拟合，可以指定权阵及圆的理论半径
        /// </summary>
        /// <param name="pts">要进行拟合的点</param>
        /// <param name="P">权阵</param>
        /// <param name="radius">圆的理论半径：
        /// 不指定或指定为0时程序自动忽略该值</param>
        /// <param name="config"><para>是否使用抗差最小二乘法</para></param>
        /// <returns>拟合所得到的圆的参数</returns>
        /// <remarks>
        /// <para>此方法用于平面圆拟合，可以指定拟合精度及圆的理论半径。</para>
        /// </remarks>
        public static FitResult Fit2D(List<Point> pts, Matrix P = null, double radius = 0, RLSConfig config = null)
        {            
            List<Point> newPts;
            Vector moveVector;
            double meanZ;
            CircleFitThings(pts, out newPts, out meanZ, out moveVector);
            FitResult result = new FitResult();
            //由于只是做初始值，所以一般不用抗差的方法，具体抗差方法后面使用
            Circle cp0 = Fit2DInFastMode(newPts, P, null).ResultGeometry as Circle;            

            Matrix retM;
            Circle retCp;
            if (radius <= 0)
            {
                Matrix m0 = new Matrix(new double[,] { { cp0.Center.X }, { cp0.Center.Y }, { cp0.Radius } });
                //表明此时不约束半径
                retM = FitPattern.LinearizedModule(newPts, m0, CircleFitFunctionModule_WithoutRadius, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config);
                retCp = new Circle(new Point(retM[0, 0], retM[1, 0], 0), retM[2, 0]);
            }
            else
            {
                //表明此时约束半径
                //注意输入的m0一定要与输出的m1维度相同
                //有时m0这个矩阵是3行1列，改成2行1列
                Matrix m0 = new Matrix(new double[,] { { cp0.Center.X }, { cp0.Center.Y } });

                retM = FitPattern.LinearizedModule(newPts, m0, CircleFitFunctionModule_WithRadius, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config, radius);
                retCp = new Circle(new Point(retM[0, 0], retM[1, 0], 0), radius);
            }
            retCp.Center += moveVector;
            retCp.Center.Z = meanZ;
            result.ResultGeometry = retCp;
            return result;
        }
                
        /// <summary>
        /// 同心圆拟合
        /// </summary>
        /// <param name="inPts">内部圆上的点</param>
        /// <param name="outPts">外部圆上的点</param>
        /// <param name="P">权阵：内部圆和外部圆合并在一起</param>
        /// <param name="radius">内部圆半径：0或负数表示不指定</param>
        /// <param name="thickness">壁厚：0表示不指定，负数表示内部圆半径较大</param>
        /// <param name="config"><para>是否使用抗差最小二乘法</para></param>
        /// <returns>拟合后的圆，结果的Parameters第一个参数为double，表示壁厚</returns>
        public static FitResult Fit2D(List<Point> inPts, List<Point> outPts, Matrix P = null, double radius = 0, double thickness = 0, RLSConfig config = null)
        {
            List<Point> newInPts, newOutPts;
            Vector moveVector;
            double meanZ;
            CircleFitThings(inPts, outPts, out newInPts, out newOutPts, out meanZ, out moveVector);
            FitResult result = new FitResult();
            Circle cp0 = Fit2DInFastMode(newInPts, P, config).ResultGeometry as Circle;
            Circle cp00 = Fit2DInFastMode(newOutPts, P, config).ResultGeometry as Circle;
            Matrix m0 = new Matrix(new double[,] { { (cp0.Center.X + cp00.Center.X) / 2 }, { (cp0.Center.Y + cp00.Center.Y) / 2 }, { cp0.Radius }, { cp00.Radius - cp0.Radius } });

            Matrix retM;
            Circle retCp;
            List<List<Point>> ptgps = new List<List<Point>>();
            ptgps.Add(newInPts);
            ptgps.Add(newOutPts);
            List<FitPattern.FunctionModuleDelegate> modulesList = new List<FitPattern.FunctionModuleDelegate>();
            double thicknessResult = 0;
            if (radius <= 0)
            {
                //表明此时不约束半径
                if (thickness == 0)
                {
                    //此时不约束半径，没有壁厚
                    modulesList.Clear();
                    modulesList.Add(CircleFitFunctionModule_WithoutRadiusWithoutThicknessInPts);
                    modulesList.Add(CircleFitFunctionModule_WithoutRadiusWithoutThicknessOutPts);
                    retM = FitPattern.GroupedModule(ptgps, m0, modulesList, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config);
                    retCp = new Circle(new Point(retM[0, 0], retM[1, 0], 0), retM[2, 0]);
                    //retCp.Thickness = retM[3, 0];
                    thicknessResult= retM[3, 0];
                }
                else
                {
                    //此时不约束半径，有壁厚
                    modulesList.Clear();
                    //注意输入的m0一定要与输出的m1维度相同
                    //有时m0这个矩阵是4行1列，改成3行1列
                    Matrix m00 = new Matrix(new double[,] { { m0[0, 0] }, { m0[1, 0] }, { m0[2, 0] } });
                    modulesList.Add(CircleFitFunctionModule_WithoutRadiusWithThicknessInPts);
                    modulesList.Add(CircleFitFunctionModule_WithoutRadiusWithThicknessOutPts);
                    retM = FitPattern.GroupedModule(ptgps, m0, modulesList, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config, 0, thickness);
                    retCp = new Circle(new Point(retM[0, 0], retM[1, 0], 0), retM[2, 0]);
                    thicknessResult = thickness;
                }
            }
            else
            {
                if (thickness == 0)
                {
                    //表明此时约束半径，没有壁厚
                    modulesList.Clear();
                    //注意输入的m0一定要与输出的m1维度相同
                    //有时m0这个矩阵是4行1列，改成3行1列
                    Matrix m00 = new Matrix(new double[,] { { m0[0, 0] }, { m0[1, 0] }, { m0[3, 0] } });
                    modulesList.Add(CircleFitFunctionModule_WithRadiusWithoutThicknessInPts);
                    modulesList.Add(CircleFitFunctionModule_WithRadiusWithoutThicknessOutPts);
                    retM = FitPattern.GroupedModule(ptgps, m0, modulesList, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config, radius);
                    retCp = new Circle(new Point(retM[0, 0], retM[1, 0], 0), radius);
                    thicknessResult = retM[2, 0];
                }
                else
                {
                    //表明此时约束半径，有壁厚
                    modulesList.Clear();
                    //注意输入的m0一定要与输出的m1维度相同
                    //有时m0这个矩阵是4行1列，改成2行1列
                    Matrix m00 = new Matrix(new double[,] { { m0[0, 0] }, { m0[1, 0] } });
                    modulesList.Add(CircleFitFunctionModule_WithRadiusWithThicknessInPts);
                    modulesList.Add(CircleFitFunctionModule_WithRadiusWithThicknessOutPts);
                    retM = FitPattern.GroupedModule(ptgps, m0, modulesList, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, null, null, config, radius, thickness);
                    retCp = new Circle(new Point(retM[0, 0], retM[1, 0], 0), radius);
                    thicknessResult = thickness;
                }
                
            }

            retCp.Center += moveVector;
            retCp.Center.Z = meanZ;
            result.ResultGeometry = retCp;
            if(ReferenceEquals(result.Parameters,null))
            {
                result.Parameters = new List<object>();                
            }else
            {
                result.Parameters.Clear();
            }
            result.Parameters.Add(thicknessResult);
            return result;
        }

        #region 私有函数
        /// <summary>
        /// 同心圆拟合内圆函数模型生成（不限定半径，不限定壁厚）
        /// </summary>
        /// <param name="pts">圆上的点</param>
        /// <param name="A">间接平差模型中的系数矩阵</param>
        /// <param name="L">间接平差模型中的系数矩阵</param>
        /// <param name="m0">初始值</param>
        /// <param name="conditions">此函数中没有用到此参数</param>
        private static void CircleFitFunctionModule_WithoutRadiusWithoutThicknessInPts(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 4);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                A[i, 2] = -1;
                A[i, 3] = 0;
                L[i, 0] = m0[2, 0] - dist;
            }
        }
        /// <summary>
        /// 同心圆拟合外圆函数模型生成（不限定半径，不限定壁厚）
        /// </summary>
        /// <param name="pts">圆上的点</param>
        /// <param name="A">间接平差模型中的系数矩阵</param>
        /// <param name="L">间接平差模型中的系数矩阵</param>
        /// <param name="m0">初始值</param>
        /// <param name="conditions">此函数中没有用到此参数</param>
        private static void CircleFitFunctionModule_WithoutRadiusWithoutThicknessOutPts(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 4);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                A[i, 2] = -1;
                A[i, 3] = -1;
                L[i, 0] = m0[2, 0] + m0[3, 0] - dist;
            }
        }


        /// <summary>
        /// 同心圆拟合内圆函数模型生成（限定半径，不限定壁厚）
        /// </summary>
        /// <param name="pts">圆上的点</param>
        /// <param name="A">间接平差模型中的系数矩阵</param>
        /// <param name="L">间接平差模型中的系数矩阵</param>
        /// <param name="m0">初始值</param>
        /// <param name="conditions">第一个参数：内圆半径</param>
        private static void CircleFitFunctionModule_WithRadiusWithoutThicknessInPts(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions)
        {
            if(conditions.Length < 1)
            {
                throw new ParametersWrongException("请内圆指定半径！");
            }
            double radius = conditions[0];
            int n = pts.Count;
            A = new Matrix(n, 3);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                A[i, 2] = 0;
                L[i, 0] = radius - dist;
            }
        }
        /// <summary>
        /// 同心圆拟合外圆函数模型生成（限定半径，不限定壁厚）
        /// </summary>
        /// <param name="pts">圆上的点</param>
        /// <param name="A">间接平差模型中的系数矩阵</param>
        /// <param name="L">间接平差模型中的系数矩阵</param>
        /// <param name="m0">初始值</param>
        /// <param name="conditions">第一个参数：内圆半径</param>
        private static void CircleFitFunctionModule_WithRadiusWithoutThicknessOutPts(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions)
        {
            if (conditions.Length < 1)
            {
                throw new ParametersWrongException("请内圆指定半径！");
            }
            double radius = conditions[0];
            int n = pts.Count;
            A = new Matrix(n, 3);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                A[i, 2] = -1;
                L[i, 0] = radius + m0[2, 0] - dist;
            }
        }

        /// <summary>
        /// 同心圆拟合内圆函数模型生成（不限定半径，限定壁厚）
        /// </summary>
        /// <param name="pts">圆上的点</param>
        /// <param name="A">间接平差模型中的系数矩阵</param>
        /// <param name="L">间接平差模型中的系数矩阵</param>
        /// <param name="m0">初始值</param>
        /// <param name="conditions">第一个参数：此函数中未用到，可以任意设置
        /// 第二个参数：同心圆的壁厚</param>
        private static void CircleFitFunctionModule_WithoutRadiusWithThicknessInPts(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 3);
            L = new Matrix(n, 1);
            //限制条件
            if (conditions.Length < 2)
            {
                throw new ParametersWrongException("请指定壁厚！");
            }
            double thickness = conditions[1];
            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                A[i, 2] = -1;
                L[i, 0] = m0[2, 0] - dist;
            }
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="pts"></param>
        /// <param name="A"></param>
        /// <param name="L"></param>
        /// <param name="m0"></param>
        /// <param name="conditions"></param>
        private static void CircleFitFunctionModule_WithoutRadiusWithThicknessOutPts(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions)
        {
            if (conditions.Length < 2)
            {
                throw new ParametersWrongException("请指定壁厚！");
            }
            double thickness = conditions[1];
            int n = pts.Count;
            A = new Matrix(n, 3);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                A[i, 2] = -1;
                L[i, 0] = m0[2, 0] + thickness - dist;
            }
        }
        
        /// <summary>
        /// 
        /// </summary>
        /// <param name="pts"></param>
        /// <param name="A"></param>
        /// <param name="L"></param>
        /// <param name="m0"></param>
        /// <param name="conditions"></param>
        private static void CircleFitFunctionModule_WithRadiusWithThicknessInPts(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions)
        {
            if (conditions.Length < 2)
            {
                throw new ParametersWrongException("请指定内径及壁厚！");
            }
            double radius = conditions[0];
            double thickness = conditions[1];
            int n = pts.Count;
            A = new Matrix(n, 2);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                L[i, 0] = radius - dist;
            }
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="pts"></param>
        /// <param name="A"></param>
        /// <param name="L"></param>
        /// <param name="m0"></param>
        /// <param name="conditions"></param>
        private static void CircleFitFunctionModule_WithRadiusWithThicknessOutPts(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions)
        {
            if (conditions.Length < 2)
            {
                throw new ParametersWrongException("请指定内径及壁厚！");
            }
            double radius = conditions[0];
            double thickness = conditions[1];
            int n = pts.Count;
            A = new Matrix(n, 2);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                L[i, 0] = radius + thickness - dist;
            }
        }
        
        private static void CircleFitThings(List<Point> pts, out List<Point> newPts, out double meanZ, out Vector moveVector)
        {
            newPts = new List<Point>();            
            moveVector = new Vector(pts[0].X, pts[0].Y, pts[0].Z);
            meanZ = 0;
            foreach (Point pt in pts)
            {
                meanZ += pt.Z;
                newPts.Add(pt - moveVector);
            }           
            meanZ = meanZ / pts.Count;
        }
        /// <summary>
        /// 同心圆拟合中常用到的一些操作的集合
        /// </summary>
        /// <param name="inPts">内圆上的点</param>
        /// <param name="outPts">外圆上的点</param>
        /// <param name="newInPts">平移之后内圆上的点</param>
        /// <param name="newOutPts">平移之后外圆上的点</param>
        /// <param name="meanZ">所有Z值的平均值</param>
        /// <param name="moveVector">平移向量</param>
        private static void CircleFitThings(List<Point> inPts, List<Point> outPts, out List<Point> newInPts, out List<Point> newOutPts, out double meanZ, out Vector moveVector)
        {
            newInPts = new List<Point>();
            newOutPts = new List<Point>();
            moveVector = new Vector(inPts[0].X, inPts[0].Y, inPts[0].Z);
            meanZ = 0;
            foreach (Point pt in inPts)
            {
                meanZ += pt.Z;
                newInPts.Add(pt - moveVector);
            }
            foreach (Point pt in outPts)
            {
                meanZ += pt.Z;
                newOutPts.Add(pt - moveVector);
            }
            meanZ = meanZ / (inPts.Count + outPts.Count);
        }
        /// <summary>
        /// 快速圆心拟合函数模型生成
        /// </summary>
        /// <param name="pts">点</param>
        /// <param name="A">系数阵</param>
        /// <param name="L">常数阵</param>
        /// <param name="m0">初始值（此例中未使用此值）</param>
        /// <param name="conditions">限制条件（此例中未使用此值）</param>
        private static void CircleFitFunctionModule_FastMode(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, params double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 3);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                A[i, 0] = pts[i].X;
                A[i, 1] = pts[i].Y;
                A[i, 2] = 1;
                L[i, 0] = -pts[i].X * pts[i].X - pts[i].Y * pts[i].Y;
            }
        }
        /// <summary>
        /// 限制半径的圆心拟合函数模型生成
        /// </summary>
        /// <param name="pts">点</param>
        /// <param name="A">系数阵</param>
        /// <param name="L">常数阵</param>
        /// <param name="m0">初始值（此例中为2行1列）</param>
        /// <param name="conditions">限制条件（此例中第1个限制条件为半径）</param>
        private static void CircleFitFunctionModule_WithRadius(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, params double[] conditions)
        {            
            //限制条件
            if (conditions.Length < 1)
            {
                throw new ParametersWrongException("请指定半径！");
            }
            int n = pts.Count;
            A = new Matrix(n, 2);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                L[i, 0] = conditions[0] - dist;
            }
        }
        /// <summary>
        /// 圆心拟合函数模型生成
        /// </summary>
        /// <param name="pts">点</param>
        /// <param name="A">系数阵</param>
        /// <param name="L">常数阵</param>
        /// <param name="m0">初始值（此例中为3行1列）</param>
        /// <param name="conditions">限制条件（此例中未使用此值）</param>
        private static void CircleFitFunctionModule_WithoutRadius(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, params double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 3);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                double dist = Math.Sqrt((pts[i].X - m0[0, 0]) * (pts[i].X - m0[0, 0]) + (pts[i].Y - m0[1, 0]) * (pts[i].Y - m0[1, 0]));
                A[i, 0] = -(pts[i].X - m0[0, 0]) / dist;
                A[i, 1] = -(pts[i].Y - m0[1, 0]) / dist;
                A[i, 2] = -1;
                L[i, 0] = m0[2, 0] - dist;                
            }
        }
        #endregion

    }
}
